/*
 * Lissajous.cpp
 *
 *  Created on: 2023年3月7日
 *      Author: Jiapeng Chen
 */

#include "Lissajous.hpp"

float TRAJECTORY_LISSAJOUS::getPos_x(float t)
{
	return offset_x + A*sin(a*t);
}

float TRAJECTORY_LISSAJOUS::getPos_y(float t)
{
	return offset_y + B*cos(b*t);
}

float TRAJECTORY_LISSAJOUS::getPos_z(float t)
{
	return offset_z + C*cos(c*t);
}

float TRAJECTORY_LISSAJOUS::getVel_x(float t)
{
	return A*a*cos(a*t + d);
}

float TRAJECTORY_LISSAJOUS::getVel_y(float t)
{
	return B*b*cos(b*t);
}

float TRAJECTORY_LISSAJOUS::getVel_z(float t)
{
	return C*c*(-sin(c*t));
}

float TRAJECTORY_LISSAJOUS::getAcc_x(float t)
{
	return A*a*a*(-sin(a*t + d));
}

float TRAJECTORY_LISSAJOUS::getAcc_y(float t)
{
	return B*b*b*(-sin(b*t));
}

float TRAJECTORY_LISSAJOUS::getAcc_z(float t)
{
	return C*c*c*(-cos(c*t));
}

void TRAJECTORY_LISSAJOUS::setOffset(float val_x,float val_y,float val_z)
{
	offset_x = val_x;
	offset_y = val_y;
	offset_z = val_z;
}

float TRAJECTORY_LISSAJOUS::getOffset_x()
{
	return offset_x;
}

float TRAJECTORY_LISSAJOUS::getOffset_y()
{
	return offset_y;
}

float TRAJECTORY_LISSAJOUS::getOffset_z()
{
	return offset_z;
}

